
/**
  ******************************************************************************
  * Copyright 2021 The grapilot Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       Sensor.c
  * @author     baiyang
  * @date       2021-7-14
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "SitlSensor/Sensor.h"
#include "mag.h"
#include "Atmtable.h"
#include "uITC/uITC.h"
#include "uITC/uITC_msg.h"
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/

/*----------------------------------variable----------------------------------*/
Sensor tSensor;

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
static void GetRCurvature(float* Rm, float* Rn, float lat);
static void ImuMode_Run(Imu* pImu, Vector3f_t* state_in, float* prop_vel, float dt);
static void MagMode_Run(Magnetometer* pMag, Location* loc, Quat_t* quat, float dt);
static void Barometer_Run(Barometer* pBro, float height, float T);
static void GPSMode_Run(GPS* pGPS, Location* loc, Vector3f_t* velocity_ef, float dt);
//初始化函数

void Sensor_Run(float dt)
{
    //订阅飞行状态数据
    uitc_digital_airplane_state tIn_State;
    itc_copy_from_hub(ITC_ID(digital_airplane_state), &tIn_State);

    //加速度计
    ImuMode_Run(&(tSensor.Accel), &(tIn_State.acc_body), tIn_State.prop_rpm, dt);

    //陀螺仪
    ImuMode_Run(&(tSensor.Gyro), &(tIn_State.gyro), tIn_State.prop_rpm, dt);

    //磁力计
    MagMode_Run(&(tSensor.Mag), &(tIn_State.location), &(tIn_State.quat), dt);

    //气压计
    Barometer_Run(&(tSensor.Baro), tIn_State.location.alt * 0.01f, dt);

    //GPS
    GPSMode_Run(&(tSensor.Gps), &(tIn_State.location), &(tIn_State.velocity_ef), dt);

    //发布数据
}
//陀螺仪和加速度计
static void ImuMode_Run(Imu* pImu, Vector3f_t* state_in, float* prop_vel,  float dt)
{
    //震动扰动
    Vector3f_t vib_value;
    VibrationMode_Update(&(pImu->vibe), &vib_value, state_in, prop_vel, dt);
    
    //偏差模型
    SensorMode_Update(&(pImu->err_mode), &(pImu->sensor_data), &vib_value, dt);
}

//磁力计模型 插值算法
static void MagMode_Run(Magnetometer* pMag, Location* loc, Quat_t* quat, float dt)
{
    Vector3f_t Mag_e, Mag_b;

    //获取惯性系下磁场强度
    Mag_e = Mag_GetEarthField(loc);

    //映射到机体轴
    quat_inv_vec_rotate(quat, &Mag_b, &Mag_e);
    
    //加上误差干扰
    SensorMode_Update(&(pMag->err_mode), &(pMag->mag_data), &Mag_b, dt);
}

//气压计传感器
static void Barometer_Run(Barometer* pBro, float height, float T)
{
    float sigma, delta, theta;
    SimpleAtmosphere(height * 0.001f, &sigma, &delta, &theta);

    float air_pressure = SSL_AIR_PRESSURE * delta;
    pBro->tempc = T;
    
    //加上偏置及噪声
    air_pressure += pBro->bias + math_rand_normal(0, pBro->variance);
    pBro->pressure_pa = air_pressure;
}

//GPS传感器
static void GPSMode_Run(GPS* pGPS, Location* loc, Vector3f_t* velocity_ef, float dt)
{
    //位置速度偏差
    Vector3f_t Pos_Error, Vel_Error;
    vec3_set(&Pos_Error, math_rand_normal(0, pGPS->pos_variance.x), 
                         math_rand_normal(0, pGPS->pos_variance.y),
                         math_rand_normal(0, pGPS->pos_variance.z));

    vec3_set(&Vel_Error, math_rand_normal(0, pGPS->vel_variance.x),
                         math_rand_normal(0, pGPS->vel_variance.y),
                         math_rand_normal(0, pGPS->vel_variance.z));

    //计算曲率半径
    float Rm, Rn;
    GetRCurvature(&Rm, &Rn, loc->lat * 1E-7f);

    //将位置偏差映射到经纬度偏差
    float dlat, dlon, dh;
    dlat = Pos_Error.x / (Rm + (loc->alt * 0.01f));
    dlon = Pos_Error.y / (Rn + (loc->alt * 0.01f) * cosf(loc->lat * 1E-7f * DEG_TO_RAD));
    dh = - Pos_Error.z / (loc->alt * 0.01f);

    int32_t lat_meas = (int32_t)(dlat * 1E7) + loc->lat;
    int32_t lon_meas = (int32_t)(dlon * 1E7) + loc->lng;
    int32_t h_meas =   (int32_t)(dh * 1E2) + loc->alt;
    
    pGPS->gps_data.lat = 0.5f * (pGPS->lat_meas) + 0.5f * lat_meas;
    pGPS->gps_data.lon = 0.5f * (pGPS->lon_meas) + 0.5f * lon_meas;
    pGPS->gps_data.alt = 0.5f * (pGPS->h_meas) + 0.5f * h_meas;

    //计算速度测量值
    Vector3f_t Vel_meas;
    vec3_add(&Vel_meas, velocity_ef, &Vel_Error);

    pGPS->gps_data.vn = (int16_t)((0.5f * Vel_meas.x + 0.5f * pGPS->vel_meas.x) * 100);
    pGPS->gps_data.ve = (int16_t)((0.5f * Vel_meas.y + 0.5f * pGPS->vel_meas.y) * 100);
    pGPS->gps_data.vd = (int16_t)((0.5f * Vel_meas.z + 0.5f * pGPS->vel_meas.z) * 100);

    //计算水平和纵向精度因子
    pGPS->gps_data.eph = (uint16_t)(vec2_length((Vector2f_t*)(&Pos_Error)) * 100);
    pGPS->gps_data.epv = (uint16_t)(fabsf(Pos_Error.z) * 100);

    //计算水平速度
    pGPS->gps_data.vel = sqrt(SQ(pGPS->gps_data.vn) + SQ(pGPS->gps_data.ve));
    //计算水平方向
    if (pGPS->gps_data.vel < 100)
    {
        pGPS->gps_data.cog = 0;
    }
    else
    {
        pGPS->gps_data.cog = (uint16_t)(atan2(pGPS->gps_data.ve, pGPS->gps_data.vn) * RAD_TO_DEG * 100);
    }
    
    if (pGPS->gps_data.cog < 0)
    {
        pGPS->gps_data.cog += 360;
    }
    pGPS->gps_data.fix_type = 3;                //3D定位
    pGPS->gps_data.statellites_visible = 10;    //搜星数

    //保存上一时刻测量值
    pGPS->lat_meas = lat_meas;
    pGPS->lon_meas = lon_meas;
    pGPS->h_meas = h_meas;
    pGPS->vel_meas = Vel_meas;
}

static void GetRCurvature(float* Rm, float* Rn, float lat)
{
    const float flat = 0.0033528f;
    float eccen2 = 2.0f * flat - flat * flat;

    float var_mid = 1 - eccen2 * SQ(sinf(lat * DEG_TO_RAD));

    *Rn = RADIUS_OF_EARTH / (sqrtf(var_mid));
    *Rm = *Rn * (1.0f - eccen2) / var_mid;
}

/*------------------------------------test------------------------------------*/


